#include "Arduino.h"
#include "settings.h"
#include "pixelbuffer.h"

#ifdef DEBUG
  #include "HardwareSerial.h"
#endif



const Letter Letter_0  = {
  '0',
  3,
  {
    B00111100,
    B01000010,
    B00111100
  }
};

const Letter Letter_1 = {
  '1',
  2,
  {
    B00000100,
    B01111110,
    B00000000
  }
};

const Letter Letter_2 = {
  '2',
  3,
  {
    B01100100,
    B01010010,
    B01001100
  }
};

const Letter Letter_3 = {
  '3',
  3,
  {
    B01000100,
    B01010010,
    B00101100
  }
};

const Letter Letter_4 = {
  '4',
  3,
  {
    B00011000,
    B01110110,
    B00010000
  }
};

const Letter Letter_5 = {
  '5',
  3,
  {
    B01011110,
    B01010010,
    B00100010
  }
};

const Letter Letter_6 = {
  '6',
  3,
  {
    B00111000,
    B01010100,
    B00100010
  }
};

const Letter Letter_7 = {
  '7',
  3,
  {
    B01110010,
    B00011010,
    B00010110
  }
};

const Letter Letter_8 = {
  '8',
  3,
  {
    B00101100,
    B01010010,
    B00101100
  }
};

const Letter Letter_9 = {
  '9',
  3,
  {
    B01001100,
    B01010010,
    B00111100
  }
};
const Letter Letter_comma = {
  ',',
  2,
  {
    B11100000,
    B01100000,
    B00000000
  }
};

const Letter Letter_degree = {
  'D',
  2,
  {
    B00000110,
    B00000110, 
    B00000000
  }
};

const Letter Letter_C = {
  'C',
  3,
  {
    B00111100,
    B01000010,
    B01000010
  }
};

const Letter Letter_F = {
  'F',
  3,
  {
    B01111110,
    B00010010,
    B00000010
  }
};

const Letter Letter_minus = {
  '-',
  3,
  {
    B00010000,
    B00010000,
    B00010000
  }
};

/*
const Letter Letter_dot = {
  '.',
  2,
  {
    B01100000,
    B01100000,
    B00000000
  }
};
const Letter Letter_colon = {
  ':',
  2,
  {
    B01101100,
    B01101100,
    B00000000
  }
};


const Letter Letter_space = {
  'S',
  3,
  {
    B00000000,
    B00000000,
    B00000000
  }
};
*/

const Letter Letter_err = {
  'X',
  3,
  {
    B00000110,
    B10100001,
    B00001110
  }
};

const Letter *all_letter_ptrs[] =
{
  &Letter_0,
  &Letter_1,
  &Letter_2,
  &Letter_3,
  &Letter_4,
  &Letter_5,
  &Letter_6,
  &Letter_7,
  &Letter_8,
  &Letter_9,
  &Letter_minus,
  &Letter_comma,
  &Letter_degree,
  &Letter_C,
  &Letter_F
  /*
  &Letter_dot,
  &Letter_colon,
  &Letter_space,
  */
  // nullptr
};

const Letter *letter_err_ptr = &Letter_err;



PixelBuffer::PixelBuffer(const PixelBuffer &other_pixbuf) {
    num_letters = other_pixbuf.num_letters;
    for (uint8_t i=0; i<pixelBufferLength; i++) {
      pixel_arr[i] = other_pixbuf.pixel_arr[i];
    }

    for (uint8_t i=0; i<maxLetters; i++) {
      letter_ptr_array[i] = other_pixbuf.letter_ptr_array[i];
    }
}



/* Returns the position of the next free element.
Will return the position of the last element, if the PixelBuffer array points to as many pointers as there are elements reserved for it's array.
*/
uint8_t PixelBuffer::get_next_letter_position(void) {
    if (this->num_letters >= maxLetters)
      return maxLetters - 1;
    return this->num_letters;
  }


/* Returns the position of last used element.
*/
uint8_t PixelBuffer::get_last_letter_position(void) {
    return this->num_letters - 1;
  }


// Clears all the letters in this->all_letter_ptrs
void PixelBuffer::clear_letters(void) {
  this->num_letters = 0;
  for(uint8_t i=0; i<maxLetters; i++) {
    this->letter_ptr_array[i] = nullptr;
  }
}


// Adds Element to end of list and returns position
uint8_t PixelBuffer::push_letter(const Letter *letter) {
  uint8_t position = this->get_next_letter_position();
  
  this->letter_ptr_array[position] = letter;

  if (position < maxLetters) {
    this->num_letters++;
  }
  return position;
}


uint8_t PixelBuffer::push_letter(const char letter) {
  #ifdef DEBUG
    Serial.println("Pushing Char: " + String(letter) + String(", int: ") + (int)letter);
  #endif

  Letter **letter_ptr = nullptr;

  for (uint8_t position=0; position < (sizeof(all_letter_ptrs)/sizeof(all_letter_ptrs[0])); position++) {
    if (all_letter_ptrs[position]->letter == letter) {
      letter_ptr = &all_letter_ptrs[position];
    }
  }

  if (letter_ptr == nullptr){
    #ifdef DEBUG
      Serial.println("Failed to push Char: '" + String(letter) + String("', int: ") + (int)letter);
    #endif
    letter_ptr = &letter_err_ptr;
  }
  
  return this->push_letter(*letter_ptr);

}


// sets new text beginning from start_position, returns end_position
void PixelBuffer::set_text(const char *text_ptr, bool append) {
  if(!append)
    this->clear_letters();

  while (*text_ptr) {
    this->push_letter(*text_ptr);
    text_ptr++;
  }
}


void PixelBuffer::set_int(int num, bool append) {
  if(!append)
    this->clear_letters();

  if (num<0) {
    this->push_letter('-');
    num *= -1;
  }
  if (num == 0) {
    this->push_letter('0');
    return;
  }

  unsigned long mask = 1;
  unsigned long temp_mask;

  while ((mask<=num) && mask) {
    temp_mask = mask;
    mask *= 10;
  }
  mask /= 10;
  if (!mask) {
    mask = temp_mask;
  }

  while (mask) {
    this->push_letter('0' + (num/mask));  // ASCII 48 is '0', 49 is '1', and so on.
    num %= mask;
    mask /= 10;
  }

}


void PixelBuffer::set_float(double num, bool append) {
  if(!append)
    this->clear_letters();
  
  if (num<0) {
    this->push_letter('-');
    num *= -1;
  }

  this->set_int((int)num, true);
  this->push_letter(DecimalSeparator);

  unsigned int decimals = (unsigned int)(num * 100);
  decimals %= 100;
  uint8_t digit;

  for (uint8_t i=0; i<2; i++) {
    digit = decimals / 10;
    decimals %= 10;
    decimals *= 10;
    this->push_letter('0' + digit);
  }
}


byte* PixelBuffer::get_buffer_array(void) {
  return this->pixel_arr;
}


void PixelBuffer::set_pixelbuffer(const byte *src_arr, uint8_t len) {
  if (len > pixelBufferLength )
    len = pixelBufferLength;
  
  for (uint8_t i=0; i<len; i++) 
    this->pixel_arr[i] = src_arr[i];
}


// draws buffer
void PixelBuffer::draw_pixelbuffer(const bool insert_blank) {
  uint8_t pixbuf_position = 0;

  this->clear_pixelbuffer();
  
  for (uint8_t i=0; i < this->num_letters; i++) {
    for (uint8_t j=0; j < this->letter_ptr_array[i]->length; j++) {
      this->pixel_arr[pixbuf_position++] = this->letter_ptr_array[i]->pixel_arr[j];
      if (pixbuf_position >= pixelBufferLength)
        return;
    }
    this->pixel_arr[pixbuf_position++] = 0b00000000;
  }
}


// invert buffer
void PixelBuffer::invert_pixelbuffer() {
  for (uint8_t i=0; i<pixelBufferLength; i++)
    this->pixel_arr[i] = this->pixel_arr[i] ^ 0b11111111;
}


// clears pixel buffer
void PixelBuffer::clear_pixelbuffer(void) {
  for (uint8_t i=0; i<pixelBufferLength; i++) {
    this->pixel_arr[i] = 0b00000000;
  }
}


#ifdef DEBUG
void PixelBuffer::print_pixelbuffer(void) {
  uint8_t mask;
  char c;

  Serial.println("---------------------------------------------------");
  for (uint8_t i=0; i<pixelBufferLength; i++) {
    mask = B10000000;
    while (mask) {
      if (mask & this->pixel_arr[i])
        c = 'X';
      else
        c = ' ';
      Serial.print(c);
      mask = mask >> 1;
    }
    Serial.println();
  }
  Serial.println("---------------------------------------------------");
}
#endif


void PixelBuffer::center_pixelbuffer(void) {
  int free_spaces = 0;

  for (uint8_t i=0; i<pixelBufferLength; i++) {
    if (this->pixel_arr[i] == 0b00000000)
      free_spaces++;
    else
      free_spaces = 0;
  }

  free_spaces /= 2;
  int position;
  for (position=pixelBufferLength-1; position>=free_spaces; position--) {
    this->pixel_arr[position] = this->pixel_arr[position - free_spaces];
  }

  while (position >= 0) {
    this->pixel_arr[position--] = 0b00000000;
  }
}


uint8_t PixelBuffer::move_pixelbuffer_left(const byte insert=B00000000) {
  uint8_t result = this->pixel_arr[0];

  for (uint8_t i=0; i<pixelBufferLength-1; i++) {
    this->pixel_arr[i] = this->pixel_arr[i+1];
  }
  this->pixel_arr[pixelBufferLength-1] = insert;

  return result;
}


uint8_t PixelBuffer::move_pixelbuffer_right(const byte insert=B00000000) {
  uint8_t result = this->pixel_arr[pixelBufferLength-1];

  for (uint8_t i=pixelBufferLength-1; i>0; i--) {
    this->pixel_arr[i] = this->pixel_arr[i-1];
  }
  this->pixel_arr[0] = insert;

  return result;
}


void PixelBuffer::move_pixelbuffer_up(void) {
  #ifdef DEBUG
    Serial.println("UP");
  #endif
  for (uint8_t i=0; i<pixelBufferLength; i++) {
    this->pixel_arr[i] >>= 1;
  }
}


void PixelBuffer::move_pixelbuffer_down(void) {
  #ifdef DEBUG
    Serial.println("DOWN");
  #endif
  for (uint8_t i=0; i<pixelBufferLength; i++) {
    this->pixel_arr[i] <<= 1;
  }
}

/*
Adds a new column to the plot.
min and max are the smallest and largest value that we want to plot in this single column
step is the range one pixel represents
*/
void PixelBuffer::insert_stat_column(float min,  // lowest occuring value
                                     float max,  // highest occuring value
                                     float step=1.0)    // stepsize which one pixel represents
{
  uint8_t result = 0;
  
  static float bottom = min;  
  
  while (min < bottom) {
    this->move_pixelbuffer_up();
    bottom -= step;
  }

  float top = bottom + (step * 8);
  while (max > top) {
    this->move_pixelbuffer_down();
    top += step;
  }

  bottom = top - (8*step);
  min -= bottom;
  max -= bottom;

  /*
  float fuzz = step / 50.0;  // have some slightly smoother transitioning between pixel values
  for(uint8_t i=0; i<8; i++) {
    float tmp_bottom = (step * i) + fuzz;
    top = (step * (i+1)) - fuzz;

    if ( (min <= top) && (max >= tmp_bottom) ) {
      result = result | 0b10000000 >> i;
    }
  }
  */

  for(uint8_t i=0; i<8; i++) {
    float tmp_bottom = (step * i);
    top = (step * (i+1));

    if ( (min <= top) && (max >= tmp_bottom) ) {
      result = result | 0b10000000 >> i;
    }
  }
  
  this->move_pixelbuffer_left(result);

  
  uint8_t bottom_space = 9;  // measures the position of the lowest pixel from the lower border of the pixelbuffer
  uint8_t top_space = 9;

  for (uint8_t i=0; i<pixelBufferLength; i++) {
    for (uint8_t j=0; j<8, j<top_space; j++) {
      if (this->pixel_arr[i] & (0b00000001 << j)) {
        if (j < top_space)
          top_space = j;
        break;
      }
    }
  }
  for (uint8_t i=0; i<pixelBufferLength; i++) {
    for (uint8_t j=0; j<8, j<bottom_space; j++) {
      if (this->pixel_arr[i] & (0b10000000 >> j)) {
        if (j < bottom_space)
          bottom_space = j;
        break;
      }
    }
  }
  if (top_space == 9) top_space = 0;
  if (bottom_space == 9) bottom_space = 0;
  
  int moves = (top_space - bottom_space) / 2;

  if (moves<0) {
    while (moves++) {
      this->move_pixelbuffer_down();
      bottom += step;
    }
  }
  else if(moves>0) {
    while (moves--) {
      this->move_pixelbuffer_up();
      bottom -= step;
    }
  }
}


/*
Create ByteBlock from pixelbuffer. 
This method returns a byteblock that was created bitwise from 8 values from the pixel-array, 
i.e. so that rows and columns are switched

  input bytes are in memory represented as counting from up to down, i.e. single bytes are 1111, 2222, etc
  output bytes represented as counting fom left to right, i.e. 1234, etc. 

      C O L U M N S

  R   1 2 3 4 5 6 7 8 9 0
  O   1 2 3 4 5 6 7 8 9 0
  W   1 2 3 4 5 6 7 8 9 0
  S   1 2 3 4 5 6 7 8 9 0

Parameters:
  - start_column: skip this many bytes in the byte-array
*/


ByteBlock PixelBuffer::get_byteblock_as_rows(const uint8_t start_column) {
  uint8_t end_column = start_column + 7;
  byte byte_arr[8] = {0};
  byte input_mask = B10000000;
  byte output_mask = B10000000;
  byte result_mask;

  if (end_column >= pixelBufferLength)
    end_column = pixelBufferLength - 1;

  for (uint8_t i=0; i<8; i++) {
    output_mask = B00000001;

    for (uint8_t j=start_column; j<=end_column; j++) {
      result_mask = this->pixel_arr[j] & input_mask;

      if (result_mask != 0) {
        byte_arr[i] = byte_arr[i] | output_mask;
      }

      output_mask <<= 1;
    }
    input_mask >>= 1;
  }
  ByteBlock result(byte_arr);
  return result;
}


ByteBlock PixelBuffer::get_byteblock_as_columns(const uint8_t start_column) {
  uint8_t num_bytes = pixelBufferLength - start_column;
  byte byte_arr[8] = {0};

  if (num_bytes > 8)
    num_bytes = 8;

  for(uint8_t pos=0; pos<num_bytes; pos++) {
    byte_arr[pos] = this->pixel_arr[start_column + pos];
  }

  ByteBlock result(byte_arr);
  return result;
}


#ifdef DEBUG
void test_pixelbuffer(void) {
  PixelBuffer pb = PixelBuffer();
  Serial.println("Testing Pixelbuffer:\n\n");

  Serial.println("Setting 123.45 via push_letter(char):");
  pb.push_letter('1');
  pb.push_letter('2');
  pb.push_letter('3');
  pb.push_letter('.');
  pb.push_letter('4');
  pb.push_letter('5');
  pb.draw_pixelbuffer();
  pb.print_pixelbuffer();

  Serial.println("\nPushing chars for which no letter exist:");
  pb.clear_letters();
  pb.push_letter('a');
  pb.push_letter('b');
  pb.push_letter('c');
  pb.push_letter(':');
  pb.draw_pixelbuffer();
  pb.print_pixelbuffer();

  pb.set_text("1488");
  pb.draw_pixelbuffer();
  Serial.println("\nSet 1488 as Text.");
  pb.print_pixelbuffer();

  Serial.println("Testing Pixelbuffer Instance Copy. Should print 1377.");
  PixelBuffer pb2(pb);
  pb2.set_text("1377");
  pb2.draw_pixelbuffer();
  pb2.print_pixelbuffer();

  Serial.println("Now everything should print 1488 again.");
  pb.center_pixelbuffer();
  Serial.println("Centering:");
  pb.print_pixelbuffer();

  pb.invert_pixelbuffer();
  Serial.println("Inverting:");
  pb.print_pixelbuffer();

  pb.set_text("14");
  pb.draw_pixelbuffer();
  Serial.println("\nSet Text to 14 and get as ByteBlock columns:");
  ByteBlock bb = pb.get_byteblock_as_columns(0);
  bb.print();

  Serial.println("\nGet as ByteBlock rows:");
  bb = pb.get_byteblock_as_rows(0);
  bb.print();

  Serial.println("\nSetting -12345 as Integer:");
  pb.set_int(-12345);
  pb.draw_pixelbuffer();
  pb.print_pixelbuffer();

  Serial.println("\nSetting -14.88 as Float:");
  pb.set_float(-14.88);
  pb.draw_pixelbuffer();
  pb.print_pixelbuffer();

  Serial.println("\nDone testing Pixelbuffer:");
}
#endif
